08. Fitting Polynomials
As we learned in the previous video, the reference trajectory is typically passed to the control block as a polynomial. This polynomial is usually 3rd order, since third order polynomials will fit trajectories for most roads. To practice this most common situation, we will learn how to fit 3rd order polynomials to waypoints (x, y) in C++ using Eigen, and evaluate the output. Your tasks are:
-
Use
polyfit
to fit a 3rd order polynomial to the given x and y coordinates representing waypoints. -
Use
polyeval
to evaluate y values of given x coordinates.
Start Quiz:
// In this quiz you'll fit a polynomial to waypoints.
#include <iostream>
#include "Dense"
using Eigen::VectorXd;
// Evaluate a polynomial.
double polyeval(const VectorXd &coeffs, double x);
// Fit a polynomial.
VectorXd polyfit(const VectorXd &xvals, const VectorXd &yvals, int order);
int main() {
VectorXd xvals(6);
VectorXd yvals(6);
// x waypoint coordinates
xvals << 9.261977, -2.06803, -19.6663, -36.868, -51.6263, -66.3482;
// y waypoint coordinates
yvals << 5.17, -2.25, -15.306, -29.46, -42.85, -57.6116;
/**
* TODO: use `polyfit` to fit a third order polynomial to the (x, y)
* coordinates.
* Hint: call Eigen::VectorXd polyfit() and pass xvals, yvals, and the
* polynomial degree/order
*/
// YOUR CODE HERE
for (double x = 0; x <= 20; ++x) {
/**
* TODO: use `polyeval` to evaluate the x values.
*/
std::cout << "YOUR CODE HERE" << std::endl;
}
// Expected output
// -0.905562
// -0.226606
// 0.447594
// 1.11706
// 1.7818
// 2.44185
// 3.09723
// 3.74794
// 4.39402
// 5.03548
// 5.67235
// 6.30463
// 6.93236
// 7.55555
// 8.17423
// 8.7884
// 9.3981
// 10.0033
// 10.6041
// 11.2005
// 11.7925
}
double polyeval(const VectorXd &coeffs, double x) {
double result = 0.0;
for (int i = 0; i < coeffs.size(); ++i) {
result += coeffs[i] * pow(x, i);
}
return result;
}
// Adapted from:
// https://github.com/JuliaMath/Polynomials.jl/blob/master/src/Polynomials.jl#L676-L716
VectorXd polyfit(const VectorXd &xvals, const VectorXd &yvals, int order) {
assert(xvals.size() == yvals.size());
assert(order >= 1 && order <= xvals.size() - 1);
Eigen::MatrixXd A(xvals.size(), order + 1);
for (int i = 0; i < xvals.size(); ++i) {
A(i, 0) = 1.0;
}
for (int j = 0; j < xvals.size(); ++j) {
for (int i = 0; i < order; ++i) {
A(j, i + 1) = A(j, i) * xvals(j);
}
}
auto Q = A.householderQr();
auto result = Q.solve(yvals);
return result;
}